#include "Intersections.h"

bool Intersections::AABBRay(float& near, float& far, const SPoint& AABBMin,
		const SPoint& AABBMax, const SPoint& rPos, const Vector& rDir)
{
	// x-slob
	if (rDir.x == .0)
	{
		if (rPos.x < AABBMin.x || rPos.x > AABBMax.x)
			return false;
	}
	else
	{
		float t1 = min((AABBMin.x - rPos.x) / rDir.x, (AABBMax.x - rPos.x)
				/ rDir.x);
		float t2 = max((AABBMin.x - rPos.x) / rDir.x, (AABBMax.x - rPos.x)
				/ rDir.x);
		near = max(near, t1);
		far = min(far, t2);
		if (near > far || far < 0.)
			return false;
	}

	// y-slob
	if (rDir.y == .0)
	{
		if (rPos.y < AABBMin.y || rPos.y > AABBMax.y)
			return false;
	}
	else
	{
		float t1 = min((AABBMin.y - rPos.y) / rDir.y, (AABBMax.y - rPos.y)
				/ rDir.y);
		float t2 = max((AABBMin.y - rPos.y) / rDir.y, (AABBMax.y - rPos.y)
				/ rDir.y);
		near = max(near, t1);
		far = min(far, t2);
		if (near > far || far < 0.)
			return false;
	}

	// z-slob
	if (rDir.z == .0)
	{
		if (rPos.z < AABBMin.z || rPos.z > AABBMax.z)
			return false;
	}
	else
	{
		float t1 = min((AABBMin.z - rPos.z) / rDir.z, (AABBMax.z - rPos.z)
				/ rDir.z);
		float t2 = max((AABBMin.z - rPos.z) / rDir.z, (AABBMax.z - rPos.z)
				/ rDir.z);
		near = max(near, t1);
		far = min(far, t2);
		if (near > far || far < 0.)
			return false;
	}
	return true;
}

float Intersections::rayTriangle(const SPoint& rayPos, const Vector& rayDir,
		const SPoint& triP1, const SPoint& triP2, const SPoint& triP3)
{
	const Vector b = triP3 - triP1;
	const Vector c = triP2 - triP1;

	const Vector Normal = Vector::crossProduct(b, c);
	const float distance = -Vector::dotProduct(rayPos - triP1, Normal)
			/ Vector::dotProduct(rayDir, Normal);

	// distance test
	if (distance < EPSILON || distance > MAXRAYDISTANCE)
		return MAXFLOAT;
	
	// determine dominant axis
	int axis;
	if (fabs(Normal.x) > fabs(Normal.y))
	{
		if (fabs(Normal.x) > fabs(Normal.z))
		axis = X;
		else
		axis = Z;
	}
	else
	{
		if (fabs(Normal.y) > fabs(Normal.z))
		axis = Y;
		else
		axis = Z;
	}

	// determine project plane axii
	const int Uaxis = modulo3[axis + 1];
	const int Vaxis = modulo3[axis + 2];

	// calculate hitpoint
	const float Pu = rayPos.get(Uaxis) + distance * rayDir.get(Uaxis) - triP1.get(Uaxis);
	const float Pv = rayPos.get(Vaxis) + distance * rayDir.get(Vaxis) - triP1.get(Vaxis);

	// check intersection
	const float a2 = (b.get(Uaxis) * Pv - b.get(Vaxis) * Pu) / (b.get(Uaxis) * c.get(Vaxis) - b.get(Vaxis) * c.get(Uaxis));

	if (a2 <= 0)
	return MAXFLOAT;

	const float a3 = (c.get(Vaxis) * Pu - c.get(Uaxis) * Pv) / (b.get(Uaxis) * c.get(Vaxis) - b.get(Vaxis) * c.get(Uaxis));
	
//	std::cout << "Slow: " << a3  << std::endl;

	if (a3 <= 0)
	return MAXFLOAT;
	if (a2 + a3 > 1)
	return MAXFLOAT;

	// ray intersects triangle
	return distance;
}

float Intersections::rayTriangleFast(const SPoint& rayPos,
		const Vector& rayDir, TriAccel& acc)
{
	// determine U and V axii
	const int uAxis = modulo3[acc.k + 1];
	const int vAxis = modulo3[acc.k + 2];

	// start high-latency division as early as possible
	const float nd = 1. / (rayDir.get(acc.k) + acc.n_u * rayDir.get(uAxis)
			+ acc.n_v * rayDir.get(vAxis));
	const float f = (acc.n_d - rayPos.get(acc.k) - acc.n_u * rayPos.get(uAxis)
			- acc.n_v * rayPos.get(vAxis)) * nd;
	
	// check for valid distance.
	if (f < EPSILON || f > MAXRAYDISTANCE)
		return MAXFLOAT;

	// compute hitpoint positions on uv plane
	const float hu = (rayPos.get(uAxis) + f * rayDir.get(uAxis));
	const float hv = (rayPos.get(vAxis) + f * rayDir.get(vAxis));
	
	// check first barycentric coordinate
	acc.u = (hv * acc.b_nu + hu * acc.b_nv + acc.b_d);
	
	if (acc.u <= 0.)
	return MAXFLOAT;
	// check second barycentric coordinate
	acc.v = (hv * acc.c_nu + hu * acc.c_nv + acc.c_d);
	
//	std::cout << "Fast: " << mue << std::endl;
	
	if (acc.v <= 0.)
	return MAXFLOAT;
	// check third barycentric coordinate
	if (acc.u + acc.v >= 1.)
	return MAXFLOAT;
	// have a valid hitpoint here. store it.
	return f;
}
